/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    Control_Node.cpp
 * @author  LJY
 * @brief   Code for .
 * @date   2022-02-28
 * @version 1.0
 * @par Change Log:
 * <table>
 * <tr><th>Date        <th>Version  <th>Author     <th>Description
 * <tr><td>2022-02-28  <td> 1.0     <td>LJY  			<td>Creator
 * </table>
 *
 ******************************************************************************
 * @attention
 *
 * if you had modified this file, please make sure your code does not have many
 * bugs, update the version Number, write dowm your name and the date, the most
 * important is make sure the users will have clear and definite understanding
 * through your new brief.
 *
 * <h2><center>&copy; Copyright (c) 2022 - ~,SCUT-RobotLab Development Team.
 * All rights reserved.</center></h2>
 ******************************************************************************
 */
#include "Control_Node.h"
#include "common/Math/orientation_tools.h"
#include "std_msgs/Int8.h"

void Control_Node::Draw_debugData(Debug_GraphType graph_type, Debug_NumType graph_num, float value, std::string name) {
    DebugData.position[graph_type * 3 + graph_num] = value;
    DebugData.name[graph_type * 3 + graph_num] = name;
}

void Control_Node::FreqTimer_Callback(const ros::TimerEvent& event) { freq_count = 0; }

void Control_Node::DebugTimer_Callback(const ros::TimerEvent& event) {
    // debugData_Puber.publish(DebugData);
}

void Control_Node::Node_Init(void) {
    // create puber and suber
    RobotCmd_Puber = n->advertise<common::RobotCommand>("/quadruped/control/command", 10);
    debugData_Puber = n->advertise<sensor_msgs::JointState>("/quadruped/qt_monitor/graph_data", 10);
    AUcmd_Puber = n->advertise<std_msgs::Int8>("/quadruped/control/au_cmd", 10);
    LegData_Suber = n->subscribe("/quadruped/control/leg_data", 100, &Control_Node::LegData_Callback, this);
    RCCmd_Suber = n->subscribe("/quadruped/control/rc_cmd", 100, &Control_Node::RCCmd_Callback, this);
    CheaterState_Suber =
        n->subscribe("/quadruped/control/cheater_state", 100, &Control_Node::CheaterState_Callback, this);
    ImuData_Suber = n->subscribe("/quadruped/control/imu_data", 100, &Control_Node::ImuData_Callback, this);
    Nav_Suber = n->subscribe("/quadruped/control/nav_data", 100, &Control_Node::Navigation_Callback, this);

    // Init TImer
    freq_timer = n->createTimer(ros::Duration(1), &Control_Node::FreqTimer_Callback, this, false, false);
    debug_timer = n->createTimer(ros::Duration(0.005), &Control_Node::DebugTimer_Callback, this, false, false);

    // create runner
    _robotRunner = new RobotRunner(new SRPQ_Controller());
    _robotRunner->init();
}

void Control_Node::Run(void) {
    ros::Rate rate(1.f / _robotRunner->dt());
    int count = 0;

    // test
    rc_control_settings rc_cmd;
    rc_cmd.mode = RC_mode::LOCOMOTION;
    rc_cmd.variable[0] = 4;    // gait 0->trot,4->stand
    rc_cmd.variable[1] = 0;    // jump_trrigle
    rc_cmd.variable[2] = 0;    // jump_type
    rc_cmd.v_des[0] = 0.0;
    rc_cmd.v_des[1] = 0.0;
    rc_cmd.omega_des[2] = 0.0;
    rc_cmd.height_variation = -0.29;
    _robotRunner->updateRC(&rc_cmd);
    RCCmd_Available = true;

    // init debug data
    for (int i = 0; i < 12; i++) {
        DebugData.position.push_back(0);
        DebugData.name.push_back("data");
    }

    // start timer
    freq_timer.start();
    debug_timer.start();

    ros::AsyncSpinner s(4);
    s.start();
    while (ros::ok()) {
        if (legdataUpdate && !_robotRunner->controlParameters.cheater_mode) {
            _robotRunner->_stateEstimator->run();
            legdataUpdate = false;
            Estimator_Available = true;
            auto& rs = _robotRunner->_stateEstimator->getResult();
        } else if (_robotRunner->controlParameters.cheater_mode) {
            Estimator_Available = true;
        }
    }
    ros::shutdown();
}

void Control_Node::updateRobotCommand(const RobotCommand* cmd) {
    float* debug = _robotRunner->getLegdebugData();
    common::RobotCommand _cmd;
    std_msgs::Int8 au_cmd;
    au_cmd.data = _au_cmd;
    if (cmd != nullptr) {
        if (iter > 1000) {
            convertToMsg(cmd, &_cmd);
            RobotCmd_Puber.publish(_cmd);
            AUcmd_Puber.publish(au_cmd);
        }
    }
    iter++;
}

void Control_Node::LegData_Callback(const common::LegDataConstPtr& msg) {
    static int init_flag = 0;
    LegData _data;
    convertFromMsg(msg, &_data);
    _robotRunner->updateData(&_data);

    if (init_flag > 20) {
        if (!LegData_Available)
            LegData_Available = true;
        legdataUpdate = true;
    } else {
        init_flag++;
    }

    if (RCCmd_Available && LegData_Available && Estimator_Available && (CheaterState_Available || ImuData_Available)) {
        updateRobotCommand(_robotRunner->run());
    }
    freq_count++;
}

void Control_Node::RCCmd_Callback(const common::RCCommandConstPtr& msg) {
    static int init_flag = 0;
    rc_control_settings _rc;
    _rc.mode = msg->mode;
    _rc.height_variation = msg->height_variation;
    for (int i = 0; i < 12; i++) {
        if (i < 2) {
            _rc.p_des[i] = msg->p_des[i];
            _rc.test_type[i] = msg->test_type[i];
        }
        if (i < 3) {
            _rc.omega_des[i] = msg->omega_des[i];
            _rc.v_des[i] = msg->v_des[i];
            _rc.rpy_des[i] = msg->rpy_des[i];
            _rc.variable[i] = msg->variable[i];
        }
        _rc.test_data[i] = msg->test_data[i];
    }

    // update au
    _au_cmd = msg->test_type[1];

    // update nav
    Nav_Enable = msg->test_type[0];
    if (!Nav_Enable)
        _robotRunner->updateRC(&_rc);

    if (!RCCmd_Available && init_flag > 20) {
        RCCmd_Available = true;
    } else
        init_flag++;
}

void Control_Node::CheaterState_Callback(const common::CheaterStateConstPtr& msg) {
    static int init_flag = 0;
    static int64_t iter = 0;
    CheaterState<float> state;
    state.orientation.w() = msg->bodyOrientation.w;
    state.orientation.x() = msg->bodyOrientation.x;
    state.orientation.y() = msg->bodyOrientation.y;
    state.orientation.z() = msg->bodyOrientation.z;

    state.acceleration << msg->acceleration[0], msg->acceleration[1], msg->acceleration[2];
    state.omegaBody << msg->omegaBody[0], msg->omegaBody[2], msg->omegaBody[1];
    state.position << msg->position[0], msg->position[1], msg->position[2];
    state.vBody << msg->vBody[0], msg->vBody[1], msg->vBody[2];
    _robotRunner->updataCheaterState(&state);
    if (!CheaterState_Available && init_flag > 20) {
        CheaterState_Available = true;
    } else
        init_flag++;
}

void Control_Node::ImuData_Callback(const common::ImuDataConstPtr& msg) {
    static int init_flag = 0;
    VectorNavData nav;
    nav.quat.w() = msg->quat.w;
    nav.quat.x() = msg->quat.x;
    nav.quat.y() = msg->quat.y;
    nav.quat.z() = msg->quat.z;

    nav.gyro << msg->gyro[0], msg->gyro[1], msg->gyro[2];
    nav.accelerometer << msg->accel[0], msg->accel[1], msg->accel[2];
    _robotRunner->updateVectorNavData(&nav);
    if (!ImuData_Available && init_flag > 20) {
        ImuData_Available = true;
    } else
        init_flag++;
}

void Control_Node::Navigation_Callback(const geometry_msgs::TwistConstPtr& msg) {
    std_msgs::Int8 cmd;
    if (Nav_Enable) {
        _robotRunner->updateRCvel(msg->linear.x, msg->linear.y, msg->angular.z);
        _robotRunner->updateRCgait(0);
    }
}

void Route_Planning::run(void) {
    auto& seResult = _robotRunner->_stateEstimator->getResult();

    // 获取当前位置
    Vec4<float> Pw;
    Pw.block(0, 0, 3, 1) = seResult.position;
    Pw(3) = 1;
    Vec4<float> Pb = T * Pw;
    float current_x = Pb(0);
    float current_y = Pb(1);
    float current_yaw = seResult.rpy(2);

    switch (process_flag) {
        case 0:
            // 首先矫正航向角
            if (fabs(target.pro_yaw - current_yaw) > 0.1) {
                _robotRunner->updateRCvel(0, 0, ((target.pro_yaw - current_yaw) > 0 ? 1. : -1.) * 0.25);
            }

            // 逼近目标位置
            else if (fabs(target.x - current_x) > MAX_ERR_X || fabs(target.y - current_y) > MAX_ERR_Y) {
                float vel_x = limit(0.15 * (target.x - current_x), -0.3, 0.3);
                float vel_y = limit(0.15 * (target.y - current_y), -0.15, 0.15);
                float omega = limit(0.15 * (target.pro_yaw - current_yaw), -0.3, 0.3);
                _robotRunner->updateRCvel(vel_x, vel_y, omega);
            } else {
                process_flag = 1;
            }
            break;
        case 1:
            // 逼近目标姿态
            if (fabs(target.yaw - current_yaw) > MAX_ERR_YAW) {
                float omega = limit(0.15 * (target.yaw - current_yaw), -0.3, 0.3);
                _robotRunner->updateRCvel(0, 0, omega);
            } else {
                process_flag = 2;
            }
            break;
        case 2:
            if(routes.size() > 0){
                // 更新目标点
                target = routes.front();
                routes.pop();

                // 更新状态
                _robotRunner->updateRCgait(0);
                process_flag = 0;
            }
            else{
                _robotRunner->updateRCgait(4);
            }
        default:
            break;
    }
}

void Route_Planning::addTarget(float* data, uint8_t type) {
    auto& seResult = _robotRunner->_stateEstimator->getResult();
    Route_Cmd cmd;
    // 直线目标点
    if (!type) {
        cmd.x = data[0];
        cmd.y = data[1];
        cmd.yaw = data[2];
        cmd.pro_yaw = seResult.rpy(2);
        routes.push(cmd);
    } 

    // 曲线目标点
    else {
        int block = data[0] / 30.;
        for(int i = 0; i < block; i ++){
            cmd.x = 0.58 * data[1];
            cmd.y = 0;
            cmd.pro_yaw = 1.57 - i * 0.79;
            cmd.yaw = cmd.pro_yaw;
        }

        // 最后修改姿态
        cmd.x = 0;
        cmd.y = 0;
        cmd.yaw = data[2];
        cmd.pro_yaw = seResult.rpy(2);
    }
}

void Route_Planning::calculateTransitionMatrix(Mat3<float> R, Vec3<float> P) {
    Mat4<float> T_temp = Mat4<float>::Zero();
    T_temp.block(0, 0, 3, 3) = R;
    T_temp.block(0, 3, 1, 3) = P;
    T_temp(3, 3) = 1;
    T = T_temp.inverse();
}

float Route_Planning::limit(float in, float min, float max) {
    if (in < min)
        return min;
    else if (in > max)
        return max;
    else
        return in;
}

int main(int argc, char** argv) {
    // node init
    ros::init(argc, argv, "Control_Node", ros::init_options::AnonymousName);
    ros::NodeHandle n;
    ros::Duration(3).sleep();

    Control_Node* ctrl_node;
    if (argc > 1) {
        if (argv[1][0] == 't') {
            ctrl_node = new Control_Node(&n, true);
        } else if (argv[1][0] == 'f') {
            ctrl_node = new Control_Node(&n, false);
        }
    } else {
        ctrl_node = new Control_Node(&n, true);
    }
    if (ctrl_node != nullptr)
        ctrl_node->Run();

    return 0;
}

/************************ COPYRIGHT(C) SCUT-RobotLab Development Team **************************/
